import math
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.lifecycle import LifecycleNode
from general_interface.action import SwitchConnection


class DisconnectTask(LifecycleNode):
    def __init__(self):
        super().__init__("local_path_planner")
        self.declare_parameter("robot1", "robot1")
        self.robot1: str = self.get_parameter("robot1").value
        self.connectorClient = ActionClient(
            self, SwitchConnection, f"/{self.robot1}/switch_connection"
        )
    
    def on_activate(self, state):
        return super().on_activate(state)

    def on_deactivate(self, state):
        return super().on_deactivate(state)
        
def main(args=None):
    rclpy.init(args=args)
    task = DisconnectTask()
    task.trigger_configure()
    rclpy.spin(task)
    rclpy.shutdown()


if __name__ == "__main__":
    main()
